#include "StdAfx.h"
#include "Hud.h"
#include <algorithm>
#include <numeric>

#define M_PI       3.14159265358979323846

static const float k_rssi_warning = 40.f;

static const float k_voltage_warning1 = 10.2f;
static const float k_voltage_warning2 = 9.9f;

static const float k_capacity_warning = 0.2f;

//////////////////////////////////////////////////////////////////////////

QColor lerp(QColor a, QColor b, float t)
{
	QColor c;
	c.setRgbF(a.redF() + (b.redF() - a.redF())*t,
		a.greenF() + (b.greenF() - a.greenF())*t,
		a.blueF() + (b.blueF() - a.blueF())*t,
		a.alphaF() + (b.alphaF() - a.alphaF())*t);
	return c;
}


//////////////////////////////////////////////////////////////////////////

Hud::Hud()
	: m_main_pen(QBrush(Qt::white), 1)
	, m_outline_pen(QBrush(Qt::black), 1)
	, m_last_tick(std::chrono::high_resolution_clock::now())
	, m_last_fps(0)
{
	m_big_font = QFont("Impact", 18);
	m_small_font = QFont("Impact", 12);

	{
		QFontMetrics fm(m_big_font);
		m_big_font_height = fm.height();
	}
	{
		QFontMetrics fm(m_small_font);
		m_small_font_height = fm.height();
	}
}

static const std::chrono::milliseconds k_delay(200);
static const std::chrono::milliseconds k_voltage_delay(2000);
static const std::chrono::milliseconds k_motor_slow_delay(20000);

void Hud::paint(QPainter& painter, QRectF rect, const Protocol::Data& data)
{
	auto now = std::chrono::high_resolution_clock::now();
	auto duration = now - m_last_time;
	m_last_time = now;

	m_rx_rssi.reset(data.rx_rssi, k_delay);
	m_rx_rssi.update(duration);

	m_tx_rssi.reset(data.tx_rssi, k_delay);
	m_tx_rssi.update(duration);

	m_voltage.reset(data.voltage, k_voltage_delay);
	m_voltage.update(duration);

	m_current.reset(data.current, k_delay);
	m_current.update(duration);

	m_capacity.reset(data.capacity, k_delay);
	m_capacity.update(duration);

	{
		float roll = data.attitude.roll < M_PI ? data.attitude.roll : -(M_PI*2.f - data.attitude.roll);
		m_roll.reset(-roll, k_delay);
		m_roll.update(duration);
	}
	{
		float pitch = data.attitude.pitch < M_PI ? data.attitude.pitch : -(M_PI*2.f - data.attitude.pitch);
		m_pitch.reset(pitch, k_delay);
		m_pitch.update(duration);
	}
	{
		float yaw = data.attitude.yaw < M_PI ? data.attitude.yaw : -(M_PI*2.f - data.attitude.yaw);
		m_yaw.reset(yaw, k_delay);
		m_yaw.update(duration);
	}

	m_altitude.reset(data.altitude, k_delay);
	m_altitude.update(duration);

	for (size_t i = 0; i < m_rc_inputs.size(); i++)
	{
		m_rc_inputs[i].reset(data.rc_input.channels[i], k_delay);
		m_rc_inputs[i].update(duration);
	}
	for (size_t i = 0; i < m_motor_outputs.size(); i++)
	{
		m_motor_outputs[i].reset(data.motors.output[i], k_delay);
		m_motor_outputs[i].update(duration);
	}
	for (size_t i = 0; i < m_motor_outputs_slow.size(); i++)
	{
		m_motor_outputs_slow[i].reset(data.motors.output[i], k_motor_slow_delay);
		m_motor_outputs_slow[i].update(duration);
	}

	QBrush default_brush;

	painter.setBrush(default_brush);
	painter.setCompositionMode(QPainter::CompositionMode_Source);

//	painter.setRenderHint(QPainter::Antialiasing);


	char voltage_text[256];
	QColor voltage_color;

	char capacity_text[256];
	QColor capacity_color;

	//battery
	{
		static const float k_text_spacing = 25;
		painter.setFont(m_big_font);
		float y = 30;
		{
			voltage_color = (m_voltage.get_value() < k_voltage_warning1) ? Qt::yellow : Qt::white;
			voltage_color = (m_voltage.get_value() < k_voltage_warning2) ? Qt::red : voltage_color;
			m_main_pen.setColor(voltage_color);

			sprintf(voltage_text, "%2.1fV", m_voltage.get_value());
			draw_text(painter, rect.topLeft() + QPointF(10, y), voltage_text, QTextOption());
			y += k_text_spacing;

			m_main_pen.setColor(Qt::white);
		}
		{
			char buf[256];
			sprintf(buf, "%2.1fA", m_current.get_value());
			draw_text(painter, rect.topLeft() + QPointF(10, y), buf, QTextOption());
			y += k_text_spacing;
		}
		{
			capacity_color = (m_capacity.get_value() < k_capacity_warning) ? Qt::red : Qt::white;
			m_main_pen.setColor(capacity_color);
			sprintf(capacity_text, "%d%%", (int)(m_capacity.get_value() * 100.f));
			draw_text(painter, rect.topLeft() + QPointF(10, y), capacity_text, QTextOption());
			y += k_text_spacing;
			m_main_pen.setColor(Qt::white);
		}
		painter.setFont(m_small_font);
	}

	//rssi
	{
		static const float k_text_spacing = 16;
		painter.setFont(m_small_font);
		char buf[256];
		float y = 120;
		{
			m_main_pen.setColor((m_rx_rssi.get_value() < k_rssi_warning) ? Qt::red : Qt::green);
			sprintf(buf, "rx:  %2.1f db", m_rx_rssi.get_value());
			draw_text(painter, rect.topLeft() + QPointF(8, y), buf, QTextOption());
			y += k_text_spacing;
		}
		{
			m_main_pen.setColor((m_tx_rssi.get_value() < k_rssi_warning) ? Qt::red : Qt::green);
			sprintf(buf, "tx:  %2.1f db", m_tx_rssi.get_value());
			draw_text(painter, rect.topLeft() + QPointF(8, y), buf, QTextOption());
			y += k_text_spacing;
		}

		m_main_pen.setColor(Qt::white);
	}

	//altitude
	{
		static const float k_pixels_per_meter = 15.f;
		const float start_x = rect.left() + rect.size().width() * 0.8f;
		const float start_y = rect.top() + rect.size().height() * 0.5f;

		float alt_r = fmodf(m_altitude.get_value(), 1.f);

		//painter.setCompositionMode(QPainter::CompositionMode_SourceOver);
		for (int i =  -20; i < 20; i++)
		{
			float y = start_y - ((float)i - alt_r) * k_pixels_per_meter;
			if (y < 0 || y > rect.bottom())
			{
				continue;
			}

			int alt_to_show = (int)m_altitude.get_value() + i;
			if (alt_to_show % 5 == 0)
			{
				draw_line(painter, QLineF(start_x - 5, y, start_x + 8, y));

				uint32_t alpha = std::min(fabsf(start_y - y) / 60.f, 1.f) * 255.f;
				alpha = alpha * alpha >> 8; //fade quicker
				alpha = alpha * alpha >> 8; //fade quicker
				QColor main_color = m_main_pen.color();
				main_color.setAlpha(alpha);
				QColor outline_color = m_outline_pen.color();
				outline_color.setAlpha(alpha);

				m_main_pen.setColor(main_color);
				m_outline_pen.setColor(outline_color);
				if (alpha < 255)
				{
					painter.setCompositionMode(QPainter::CompositionMode_SourceOver);
				}
				//draw text
				{
					char buf[256];
					sprintf(buf, "%dm", alt_to_show);
					draw_text(painter, QPointF(start_x + 14, y), buf, QTextOption(Qt::AlignVCenter));
				}
				painter.setCompositionMode(QPainter::CompositionMode_Source);
				main_color.setAlpha(255);
				outline_color.setAlpha(255);
				m_main_pen.setColor(main_color);
				m_outline_pen.setColor(outline_color);
			}
			else
			{
				draw_line(painter, QLineF(start_x - 5, y, start_x + 3, y));
			}
		}


		draw_line(painter, QLineF(start_x + 10, start_y, start_x + 90, start_y));

		{
			char buf[256];
			sprintf(buf, "%4.1fm", m_altitude.get_value());
			draw_text(painter, QPointF(start_x + 50, start_y - 2), buf, QTextOption());
		}
		//painter.setPen(stroke_pen);
	}

	//rc input, channels 1, 2, 4
// 	{
// 		static const float k_width = 40.f;
// 		static const float k_height = 40.f;
// 		const float start_x = rect.left() + 15.f;
// 		const float start_y = rect.top() + rect.size().height() - k_height - 10.f;
// 
// 		//draw rc input throttle
// 		{
// 			float v = m_rc_inputs[2].get_value() * k_height;
// 			painter.fillRect(QRectF(start_x + k_width, start_y + k_height - v, 5, v + 1), Qt::red);
// 			draw_rect(painter, QRectF(start_x + k_width, start_y, 5, k_height));
// 		}
// 		//draw rc input yaw
// 		{
// 			float v = (m_rc_inputs[3].get_value() - 0.5f) * k_width;
// 			painter.fillRect(QRectF(start_x + k_width*0.5f, start_y + k_height, v, 5), Qt::blue);
// 			draw_rect(painter, QRectF(start_x, start_y + k_height, k_width, 5));
// 		}
// 
// 		//draw rc input pitch/roll
// 		{
// 			float x = m_rc_inputs[0].get_value()*k_width;
// 			float y = (1.f - m_rc_inputs[1].get_value())*k_height;
// 			draw_line(painter, QLineF(start_x + x, start_y, start_x + x, start_y + k_height));
// 			draw_line(painter, QLineF(start_x, start_y + y, start_x + k_width, start_y + y));
// 
// 			draw_rect(painter, QRectF(start_x, start_y, k_width, k_height));
// 
// 			painter.setBrush(QBrush(Qt::black));
// 			painter.drawEllipse(QPointF(start_x + x, start_y + y), 3, 3);
// 			painter.setBrush(default_brush);
// 		}
// 
// 		//rc input, channels 3, 5, 6, 7, 8
// 		{
// 			static const float k_width = 8.f;
// 			static const float k_height = 35.f;
// 			static const float k_spacing = 2.f;
// 			const float start_x = rect.left() + 80.f;
// 			const float start_y = rect.top() + rect.size().height() - k_height - 10.f;
// 
// 			float x = start_x;
// 			for (size_t i = 4; i < m_rc_inputs.size(); i++)
// 			{
// 				float v = m_rc_inputs[i].get_value() * k_height;
// 				painter.fillRect(QRectF(x, start_y + k_height - v, k_width + 1, v + 1), Qt::yellow);
// 
// 				draw_rect(painter, QRectF(x, start_y, k_width, k_height));
// 
// 				x += k_width + k_spacing;
// 			}
// 		}
// 	}
	{
		//roll, pitch, yaw
		{
			//auto old_render_hints = painter.renderHints();
			//painter.setRenderHint(QPainter::Antialiasing);

			static const float k_line_length = 120.f;
			static const float k_div_line_length = 60.f;
			static const float k_div_spacing = 50.f;
			const float start_x = rect.left() + rect.size().width() * 0.5f;
			const float start_y = rect.top() + rect.size().height() * 0.5f;

			float sinr = sinf(m_roll.get_value());
			float cosr = cosf(m_roll.get_value());

			float tanp = tanf(m_pitch.get_value());

			for (int i = -15; i < 15; i++)
			{
				float x = start_x + sinr*(i * k_div_spacing);
				float y = start_y + tanp * 200.f - cosr*(i * k_div_spacing);
				if (y < 0 || y > rect.bottom())
				{
					continue;
				}

				m_main_pen.setStyle(Qt::SolidLine);

				if (y < 150 || y > rect.bottom() - 150)
				{
					continue;
				}

				if (i == 0)
				{
					QLineF line(QPointF(x - cosr*k_line_length, y - sinr*k_line_length), 
						QPointF(x + cosr*k_line_length, y + sinr*k_line_length));

					{
						m_main_pen.setColor(voltage_color);
						draw_text(painter, line.p1() + QPointF(4, 20), voltage_text, QTextOption());
						m_main_pen.setColor(Qt::white);
					}
					{
						m_main_pen.setColor(capacity_color);
						draw_text(painter, line.p2() + QPointF(-4, 20), capacity_text, QTextOption(Qt::AlignRight));
						m_main_pen.setColor(Qt::white);
					}

					//painter.setPen(stroke_pen);
					draw_line(painter, line);

					float dx = line.unitVector().dx();
					float dy = line.unitVector().dy();

					const int gradations = 10;
					const float gradation_pixels = k_line_length * 2.f / gradations;
					const float yaw_gradation = 10.f; //every 10 degrees
					for (int j = 1; j <= gradations; j++)
					{
						float yaw_degrees = m_yaw.get_value() * 180.f / M_PI;
						float r = fmodf(yaw_degrees, yaw_gradation) / yaw_gradation * gradation_pixels;

						QPointF offset(dx*j*gradation_pixels + r*dx, dy*j*gradation_pixels + r*dy);
						draw_line(painter, QLineF(line.p1() + offset, line.p1() + offset + QPointF(sinr*5.f, -cosr*5.f)));
					}
				}
				else
				{
					QLineF line(x - cosr*k_div_line_length, y - sinr*k_div_line_length, 
						x + cosr*k_div_line_length, y + sinr*k_div_line_length);

					m_main_pen.setStyle(Qt::DashLine);
					//painter.setPen(pen);
					draw_line(painter, line);
				}
			}

			m_main_pen.setStyle(Qt::SolidLine);
			//painter.setPen(stroke_pen);
			//painter.setRenderHints(old_render_hints);
		}

// 		float x = start_x;
// 		for (size_t i = 4; i < m_rc_inputs.size(); i++)
// 		{
// 			float v = m_rc_inputs[i].get_value() * k_height;
// 			painter.fillRect(QRectF(x, start_y + k_height - v, k_width + 1, v + 1), Qt::yellow);
// 
// 			draw_rect(painter, QRectF(x, start_y, k_width, k_height));
// 
// 			x += k_width + k_spacing;
// 		}
	}

	{
		auto now = std::chrono::high_resolution_clock::now();
		auto duration = now - m_last_tick;
		m_last_tick = now;
		auto durationf = static_cast<float>(std::chrono::duration_cast<std::chrono::milliseconds>(duration).count());
		float fps = durationf > 1.f ? 1000.f / durationf : 1000.f;
		fps = (m_last_fps + fps) * 0.5f;
		m_last_fps = fps;

		char buf[32];
		sprintf(buf, "%2.1f", fps);
		painter.setFont(m_small_font);
		draw_text(painter, rect.topRight() + QPointF(-40, 18), buf, QTextOption());
	}

	float averaged_motor_output = 0;
	//motor info
	{
		for (auto& x: m_motor_outputs)
		{
			averaged_motor_output += x.get_value();
		}
		averaged_motor_output /= (float)m_motor_outputs.size();

		const float size = 40.f;
		const float x = rect.left() + 70.f;
		const float y = rect.bottom() - 70.f;

		QLineF line1(QPointF(x + size, y - size), QPointF(x - size, y + size));
		QLineF line2(QPointF(x - size, y - size), QPointF(x + size, y + size));

		draw_line(painter, line1);
		draw_line(painter, line2);

		draw_motor(painter, line1.p1(), 0);
		draw_motor(painter, line1.p2(), 1);
		draw_motor(painter, line2.p1(), 2);
		draw_motor(painter, line2.p2(), 3);
		{
			QColor color = lerp(QColor(Qt::green), QColor(Qt::red), averaged_motor_output);
			painter.setBrush(QBrush(color));
			painter.drawEllipse(QPointF(x, y), 10, 10);
		}
	}

	//status
	{
		char buf[32];

		if (data.status.is_armed && averaged_motor_output < 0.1f)
		{
			painter.setFont(m_small_font);
			draw_text(painter, QPointF(rect.center().x(), rect.top() + 20), "** ARMED **", QTextOption(Qt::AlignHCenter));
		}
	}
}

void Hud::draw_line(QPainter& painter, const QLineF& line)
{
	m_outline_pen.setWidthF(3);
 	painter.setPen(m_outline_pen);
 	painter.drawLine(line);
	m_outline_pen.setWidthF(1);
	painter.setPen(m_main_pen);
	painter.drawLine(line);
}

void Hud::draw_rect(QPainter& painter, const QRectF& rect)
{
	m_outline_pen.setWidthF(3);
	painter.setPen(m_outline_pen);
	painter.drawRect(rect);
	m_outline_pen.setWidthF(1);
	painter.setPen(m_main_pen);
	painter.drawRect(rect);
}

void Hud::draw_text(QPainter& painter, const QPointF& point, const char* text, QTextOption option)
{
	QFontMetrics fm(painter.fontMetrics());
	auto rect = QRectF(fm.tightBoundingRect(text));
	QPointF offset;
	if (option.alignment() & Qt::AlignVCenter)
	{
		offset.setY(fm.strikeOutPos() / 2.f);
	}
	if (option.alignment() & Qt::AlignHCenter)
	{
		offset.setX(rect.width() * -0.5f);
	}
	else if (option.alignment() & Qt::AlignRight)
	{
		offset.setX(-rect.width());
	}

	//painter.setPen(m_outline_pen);
// 	painter.drawText(point + offset + QPointF(1, 0), text);
// 	painter.drawText(point + offset + QPointF(-1, 0), text);
// 	painter.drawText(point + offset + QPointF(0, 1), text);
// 	painter.drawText(point + offset + QPointF(0, -1), text);
//	painter.drawText(point + offset + QPointF(2, 2), text);

	{
		auto ocm = painter.compositionMode();
		painter.setCompositionMode(QPainter::CompositionMode_SourceOver);

		QColor bg(Qt::black);
		bg.setAlpha(100 * m_main_pen.color().alpha() >> 8);
		QBrush brush(bg);
		rect = QRectF(rect.topLeft() + QPointF(-1, -1), rect.bottomRight() + QPointF(2, 1));
		rect.translate(point + offset);
		painter.fillRect(rect, brush);

		painter.setCompositionMode(ocm);
	}

	painter.setPen(m_main_pen);
	painter.drawText(point + offset, text);
}

//////////////////////////////////////////////////////////////////////////

void Hud::draw_motor(QPainter& painter, const QPointF& pos, size_t motor_idx)
{
	QColor color = lerp(QColor(Qt::green), QColor(Qt::red), m_motor_outputs_slow[motor_idx].get_value());
	painter.setBrush(QBrush(color));
	painter.drawEllipse(pos, 14, 14);
	color = lerp(QColor(Qt::green), QColor(Qt::red), m_motor_outputs[motor_idx].get_value());
	painter.setBrush(QBrush(color));
	painter.drawEllipse(pos, 8, 8);
}
